/*
 * Copyright 2021 <copyright holder> <email>
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 *     http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "camera.h"

namespace myslam{
    Camera::Camera(){}
    
    Vec3 Camera::world2camera(const Vec3 &p_w, const SE3 &T_c_w)
    {
        return pose_*T_c_w*p_w;
    }
    
    Vec3 Camera::camera2world(const Vec3& p_c, const SE3& T_c_w)
    {
        return T_c_w.inverse()*pose()*p_c;
    }
    
    Vec2 Camera::camera2pixel(const Vec3& p_c)
    {
        return Vec2(
            fx_*p_c(0,0)/p_c(0,2)+cx_,
            fy_*p_c(0,1)/p_c(0,2)+cy_
        );
    }
    
    Vec3 Camera::pixel2camera(const Vec2& p_p, double depth)
    {
        return Vec3(
            (p_p(0,0)-cx_)*depth/fx_,
            (p_p(1,0)-cx_)*depth/fx_,
            depth
               );
    }
    
    Vec2 Camera::world2pixel(const Vec3& p_w, const SE3& T_c_w)
    {
        return camera2pixel(world2camera(p_w, T_c_w));
    }
    
    Vec3 Camera::pixel2world(const Vec2& p_p, const SE3& T_c_w, double depth)
    {
        return camera2world(pixel2camera(p_p, depth), T_c_w);
    }
}
    
